#ifndef __XC330_MOTOR_H__
#define __XC330_MOTOR_H__

#include "main.h"
#include "bsp_dwt.h"
#include "bsp_usart.h"
#include "xc330_reg.h"

#define Header1 0xFF
#define Header2 0xFF
#define Header3 0xFD
#define Header4 0x00 

/* Drive Mode */
typedef enum
{
    Normal_Mode = 0,  // CW
    Reverse_Mode = 1, // CCW
    Velocity_based_Profile = 0x00,
    Time_based_Profile = 0x04,
    Torque_On = 0x08,
    Torque_Off = 0x00,
} Drive_Mode_e;
/* Operating Mode */
typedef enum
{
    Current_Control_Mode = 0,
    Velocity_Control_Mode = 1,
    Position_Control_Mode = 3,
    Extended_Position_Control_Mode = 4,
    Current_based_Position_Control_Mode = 5,
    PWM_Control_Mode = 16,
} Operating_Mode_e;

typedef struct
{
    float present_position;
    float present_velocity;
    float present_current;
    float present_input_voltage;
    float present_temperature;
}XC330_measure_s;

typedef struct
{

    uint8_t id;
    Drive_Mode_e drive_mode;
    Operating_Mode_e operating_mode;
    float velocity_Kp;
    float velocity_Ki;
    float velocity_limit;
} XC330_config_s;

typedef struct
{
    uint8_t id;
    Drive_Mode_e drive_mode;
    Operating_Mode_e operating_mode;
    float velocity_Kp;
    float velocity_Ki;
    float velocity_limit;
    XC330_measure_s measure;
    USARTInstance *usart_instance;
} XC330_Instance_s;

XC330_Instance_s *XC330_Register(XC330_config_s *config);
void XC330_Decode(XC330_Instance_s *instance);
void XC330_WriteEnable(void);
void XC330_WriteDisable(void);
void XC330_SetLedOn(XC330_Instance_s *instance);
void XC330_SetLedOff(XC330_Instance_s *instance);
void XC330_SetVelocity(XC330_Instance_s *instance, int32_t velocity);
void XC330_SetID(XC330_Instance_s *instance, uint8_t id);
void XC330_Read(XC330_Instance_s *instance, uint8_t address, uint8_t len);
void XC330_Write(XC330_Instance_s *instance, uint8_t address, uint8_t *data, uint8_t len);
void XC330_SetTorqueMode(XC330_Instance_s *instance, uint8_t torque_mode);
void XC330_SetDriveMode(XC330_Instance_s *instance, Drive_Mode_e drive_mode);
void XC330_SetOperatingMode(XC330_Instance_s *instance, Operating_Mode_e operating_mode);
void XC330_SetProfileAcceleration(XC330_Instance_s *instance, uint32_t acceleration);
#endif // __XC330_MOTOR_H__